﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace WorldSimulator
{
	/// <summary>
	/// Represents a vector in 3D-Space
	/// </summary>
	public class MyVector3D
	{
		//// CONSTRUCTION

		/// <summary>
		/// Zero-Vector
		/// </summary>
		public MyVector3D()
		{
			this.X = 0;
			this.Y = 0;
			this.Z = 0;
		}

		/// <summary>
		/// Vector where Z-Value is zero
		/// </summary>
		/// <param name="X">X-Value</param>
		/// <param name="Y">Y-Value</param>
		public MyVector3D(double X, double Y)
		{
			this.X = X;
			this.Y = Y;
			this.Z = 0;
		}

		/// <summary>
		/// Vector with full initialization
		/// </summary>
		/// <param name="X">X-Value</param>
		/// <param name="Y">Y-Value</param>
		/// <param name="Z">Z-Value</param>
		public MyVector3D(double X, double Y, double Z)
		{
			this.X = X;
			this.Y = Y;
			this.Z = Z;
		}

		//// PUBLIC R/W - Properties

		/// <summary>
		/// The vector's X-value
		/// </summary>
		public double X { get; set; }

		/// <summary>
		/// The vector's Y-value
		/// </summary>
		public double Y { get; set; }

		/// <summary>
		/// The vector's Z-value
		/// </summary>
		public double Z { get; set; }

		//// OPERATORS

		public static MyVector3D operator +(MyVector3D v1, MyVector3D v2)
		{
			return new MyVector3D(v1.X + v2.X, v1.Y + v2.Y, v1.Z + v2.Z);
		}

		public static MyVector3D operator -(MyVector3D v1, MyVector3D v2)
		{
			return new MyVector3D(v1.X - v2.X, v1.Y - v2.Y, v1.Z - v2.Z);
		}

		public static MyVector3D operator *(MyVector3D v1, MyVector3D v2)
		{
			return new MyVector3D(v1.X * v2.X, v1.Y * v2.Y, v1.Z * v2.Z);
		}

		public static MyVector3D operator *(MyVector3D v1, double x)
		{
			return new MyVector3D(v1.X * x, v1.Y * x, v1.Z * x);
		}

		public static MyVector3D operator /(MyVector3D v1, MyVector3D v2)
		{
			return new MyVector3D(v1.X / v2.X, v1.Y / v2.Y, v1.Z / v2.Z);
		}

		public static MyVector3D operator /(MyVector3D v1, double x)
		{
			return new MyVector3D(v1.X / x, v1.Y / x, v1.Z / x);
		}

		//// PUBLIC METHODS

		/// <summary>
		/// Calculates the distance to another vector
		/// </summary>
		public double DistanceTo(MyVector3D toVector)
		{
			return Math.Sqrt(Math.Pow(toVector.X - this.X, 2) + Math.Pow(toVector.Y - this.Y, 2) + Math.Pow(toVector.Z - this.Z, 2));
		}

		/// <summary>
		/// Returns a normalized copy
		/// </summary>
		public MyVector3D ToNormalized()
		{
			double Len = this.Length;
			return new MyVector3D(X * Len, Y * Len, Z * Len);
		}

		/// <summary>
		/// Calculates the length of the vector
		/// </summary>
		public double Length
		{
			get
			{
				return Math.Sqrt(X * X + Y * Y + Z * Z);
			}

			set
			{
				double Len = this.Length;
				X = X / Len * value;
				Y = Y / Len * value;
				Z = Z / Len * value;
			}
		}

		/// <summary>
		/// String representation
		/// </summary>
		public override string ToString()
		{
			return string.Format("({0}/{1}/{2}) Length: {3}", X, Y, Z, Length);
		}

		/// <summary>
		/// Rotates the vector
		/// </summary>
		public void Rotate(double angleDeg, double x, double y, double z)
		{
			Rotate(new MyRotation3D(angleDeg.DegToRad(), new MyVector3D(x, y, z)));
		}

		/// <summary>
		/// Rotates the vector
		/// </summary>
		/// <param name="rot"></param>
		public void Rotate(MyRotation3D rot)
		{
			double a = (Math.Cos(rot.Angle) + Math.Pow(rot.Axis.X, 2) * (1 - Math.Cos(rot.Angle)));
			double b = (rot.Axis.X * rot.Axis.Y * (1 - Math.Cos(rot.Angle)) - rot.Axis.Z * Math.Sin(rot.Angle));
			double c = (rot.Axis.X * rot.Axis.Y * (1 - Math.Cos(rot.Angle)) + rot.Axis.Y * Math.Sin(rot.Angle));

			double d = (rot.Axis.Y * rot.Axis.X * (1 - Math.Cos(rot.Angle)) + rot.Axis.Z * Math.Sin(rot.Angle));
			double e = (Math.Cos(rot.Angle) + Math.Pow(rot.Axis.Y, 2) * (1 - Math.Cos(rot.Angle)));
			double f = (rot.Axis.Y * rot.Axis.Z * (1 - Math.Cos(rot.Angle) - rot.Axis.X * Math.Sin(rot.Angle)));

			double g = (rot.Axis.Z * rot.Axis.X * (1 - Math.Cos(rot.Angle) - rot.Axis.Y * Math.Sin(rot.Angle)));
			double h = (rot.Axis.Z * rot.Axis.Y * (1 - Math.Cos(rot.Angle) + rot.Axis.X * Math.Sin(rot.Angle)));
			double i = (Math.Cos(rot.Angle) + Math.Pow(rot.Axis.Z, 2) * (1 - Math.Cos(rot.Angle)));

			double x = a * this.X + b * this.Y + c * this.Z;
			double y = d * this.X + e * this.Y + f * this.Z;
			double z = g * this.X + h * this.Y + i * this.Z;

			this.X = x;
			this.Y = y;
			this.Z = z;
		}

		/// <summary>
		/// Returns a rotated copy
		/// </summary>
		/// <param name="rot"></param>
		/// <returns></returns>
		public MyVector3D AsRotated(MyRotation3D rot)
		{
			var copy = (MyVector3D)this.MemberwiseClone();
			copy.Rotate(rot);
			return copy;
		}
	}


}
